from PyDaq.Hardware.Avr import AvrShell
from PyDaq.Utils.Debug import dbg
import simplejson as SJ 
from pylab import *
import time
import re
_ip_addr = re.compile('\d{1,3}\.\d{1,3}\.\d{1,3}\.\d{1,3}')

class ProtoMO(AvrShell):
    '''
    classdocs
    '''
    
    FORWARD  = 1
    REVERSE  = 0
    debug    = 0
    idn      = ''
    
    def __init__(self, address):
        dbg(self, 1, 'def: __init__(self)')        
        AvrShell.__init__(self, address)
        
    def __del__(self):
        ''' 
        Destructor        
        '''
        
    def __unicode__(self):
        return "ProtoMO"
    
    def GetIdn(self):
        idn      = self.ParseCommand('I','idn')
        self.idn = idn        
        return idn
    
    def Move(self, pwm, dir, motor):
        cmd = "pwm %d %d %d" % (motor, dir, pwm)
        print cmd
        Mo.send(cmd)
    
    
    def GetADC(self, volt=1): 
        dbg(self, 1, 'def GetADC(self)')
        raw  = self.query("adc", "</a>", "a")
        if not raw[0]:
            adc  = SJ.loads(raw[1])
            status = 0
        else:
            dbg(self, 1, 'ERROR IN: def GetADC(self) - did nor recive expected response to adc query')
            adc = []
            status = 1
#        for i in raw:
#            val  = int(i[1])
#            if volt==1:
#                dbg(self,2,'Converting to voltage: {0} = {1}V'.format(i[1],val))                
#                val = float(val*5)/1024
#            adc.append([i[0], val])
        return (status, adc)
     
    def ParseCommand(self, cmd, tag1='',tag2=''):
        dbg(self,1,'def ParseCommand(self, {0}, tag1={1}, tag2={2}):'.format(cmd,tag1,tag2))
        raw       = self.query(cmd,tag=tag1)        
        pattern   = re.compile(r'(?:<{0} \w+=")(.*)(?:">)(.*)(?:</{0}>)'.format(tag2))
                
        if len(tag2) > 0:            
            out  = pattern.findall(raw)
            if len(out) > 0:
                dbg(self,2,"Recovered tag2: {0}".format(tag2))             
                return out
            else:
                dbg(self,2,"Did not recovered tag2: {0}".format(tag2))
                return ''    
        else:
            return raw
    def Test(self):
        pass
        
def str2num(str):
    out = [float(x) for x in str.split()]
    return out

if __name__ == '__main__':
    print "================================"
    print "ProtoMO"
    
    basics     = 1
    locomotion = 0
    sensors    = 1
    Mo = ProtoMO(14)
    
    
    if basics:
        print Mo.GetIdn()
    
    if locomotion:
        Mo.Move(100, 0, 1)
        Mo.Move(100, 0, 2)
        time.sleep(1)
        
        Mo.Move(100, 1, 1)
        Mo.Move(100, 1, 2)
        time.sleep(1)
        
        Mo.Move(100, 0, 1)
        Mo.Move(100, 1, 2)
        time.sleep(1)
        
        Mo.Move(100, 1, 1)
        Mo.Move(100, 0, 2)
        time.sleep(1)
        
        Mo.Move(255, 1, 1)
        Mo.Move(255, 1, 2)
        time.sleep(1)
    
    if sensors:
        data = []
        for i in range(20):
            results = Mo.GetADC()
            print results
            if not results[0]:
                data.append(results[1])
        plot(data)
        show()
            
        
    Mo.Move(0, 0, 1)
    Mo.Move(0, 0, 2)
    print "All Done"